#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
from move_base_msgs.msg import MoveBaseActionGoal
from nav_msgs.msg import Path
from std_msgs.msg import Float32
import time
from tabulate import tabulate
import math

class RvizGoalMonitor:
    def __init__(self):
        rospy.init_node('rviz_goal_monitor')
        
        # 订阅器
        self.goal_sub = rospy.Subscriber('/move_base/goal', MoveBaseActionGoal, self.goal_callback)
        self.path_sub = rospy.Subscriber('/move_base/NavfnROS/plan', Path, self.path_callback)
        
        # 发布器
        self.time_pub = rospy.Publisher('/planning_time_ms', Float32, queue_size=10)
        
        # 测试参数
        self.max_attempts = 150
        self.attempt_count = 0
        self.results = []
        
        # 状态变量
        self.current_attempt = None
        rospy.loginfo(f"=== RViz目标点监控开始 (最大{self.max_attempts}次) ===")

    def goal_callback(self, msg):
        """检测新目标点"""
        if self.current_attempt is not None:
            rospy.logwarn(f"忽略新目标，测试#{self.current_attempt['id']}进行中")
            return
            
        self.current_attempt = {
            'id': self.attempt_count + 1,
            'start_time_ms': time.time() * 1000,
            'goal_pos': (msg.goal.target_pose.pose.position.x,
                        msg.goal.target_pose.pose.position.y),
            'planning_time_ms': None,
            'path_length': None,
            'path_points': 0,
            'success': False
        }
        rospy.loginfo(f"=== 检测到新目标点 #{self.current_attempt['id']} ===")

    def path_callback(self, msg):
        """处理路径规划结果"""
        if self.current_attempt is None or self.current_attempt['path_points'] > 0:
            return
            
        # 计算规划耗时(毫秒)
        current_time_ms = time.time() * 1000
        planning_time_ms = current_time_ms - self.current_attempt['start_time_ms']
        
        # 计算路径长度
        path_length = sum(
            math.hypot(
                p1.pose.position.x - p2.pose.position.x,
                p1.pose.position.y - p2.pose.position.y
            )
            for p1, p2 in zip(msg.poses[:-1], msg.poses[1:])
        )
        
        # 更新当前尝试
        self.current_attempt.update({
            'planning_time_ms': planning_time_ms,
            'path_length': path_length,
            'path_points': len(msg.poses),
            'success': len(msg.poses) >= 2  # 有效路径至少2个点
        })
        
        # 发布规划时间
        self.time_pub.publish(Float32(planning_time_ms))
        
        # 打印结果
        self.print_result()
        
        # 记录结果
        self.results.append(self.current_attempt)
        self.attempt_count += 1
        self.current_attempt = None
        
        # 检查完成条件
        if self.attempt_count >= self.max_attempts:
            rospy.signal_shutdown("测试完成")

    def print_result(self):
        """打印单次结果"""
        attempt = self.current_attempt
        rospy.loginfo(f"""
        [#{attempt['id']}] 规划结果:
        ├─ 状态: {'成功' if attempt['success'] else '失败'}
        ├─ 耗时: {attempt['planning_time_ms']:.2f} ms
        ├─ 长度: {attempt['path_length']:.3f} m
        └─ 点数: {attempt['path_points']}""")
        
        # 实时统计
        if self.results:
            success_rate = 100 * sum(r['success'] for r in self.results) / len(self.results)
            rospy.loginfo(f"当前统计: 成功率 {success_rate:.1f}% ({len(self.results)}/{self.max_attempts})")

    def print_summary(self):
        """打印最终汇总报告"""
        if not self.results:
            rospy.logwarn("无有效测试数据")
            return
            
        # 准备表格数据
        table = [["ID", "状态", "耗时(ms)", "长度(m)", "路径点数"]]
        for r in self.results:
            table.append([
                r['id'],
                "成功" if r['success'] else "失败",
                f"{r['planning_time_ms']:.2f}",
                f"{r['path_length']:.3f}",
                r['path_points']
            ])
        
        # 计算统计
        success_rate = 100 * sum(r['success'] for r in self.results) / len(self.results)
        
        # 打印报告
        print("\n" + "="*64)
        print("          RViz目标点规划汇总报告")
        print("="*64)
        print(tabulate(table, headers="firstrow", tablefmt="grid"))
        print(f"\n成功率: {success_rate:.1f}% ({len(self.results)}次有效测试)")
        print("="*64)

    def run(self):
        rospy.on_shutdown(self.print_summary)
        rospy.spin()

if __name__ == '__main__':
    try:
        monitor = RvizGoalMonitor()
        monitor.run()
    except rospy.ROSInterruptException:
        pass